#include "dispatch.h"
#include <algorithm>
#include <iomanip>

using namespace CANalyst2;
std::map<std::string, std::function<std::shared_ptr<CANProcess>()>> *CANProcess::impls;
std::vector<std::shared_ptr<CANDispatch>> CANProcess::cans;
int can_index = 0;

// 静态单例 UsbCAN，用于多通道共享
static std::shared_ptr<UsbCAN> usbcan_singleton = nullptr;

CANDispatch::CANDispatch(const std::string &dev, CanBaud baud, int can_ind)
{
    index = can_index++;
    buffer_send0.reserve(128);
    buffer_send1.reserve(128);
    is_valid = false;

    // 只创建一次 UsbCAN 对象
    if (dev.find("usbcan") == 0) {
        if (!usbcan_singleton) {
            usbcan_singleton = std::make_shared<UsbCAN>();
        }
        can = usbcan_singleton;
    } 
    else if (dev.find("socketcan") == 0) {
        can = std::make_shared<SocketCAN>();
    }

    if (!can || !can->get_can_usb_opened()) {
        can = nullptr;
        std::cerr << "USB CAN open failed!\n";
        return;
    }

    if (can->open(baud, can_ind) != 0) {
        can = nullptr;
        std::cerr << "CAN channel " << can_ind << " init failed\n";
        return;
    }

    is_valid = true;
    coe = std::make_shared<CANOPEN>(can);
}

CANDispatch::~CANDispatch()
{
  if (!can) {
    return;
  }
  VCI_ResetCAN(VCI_USBCAN2, 0, 0);
  usleep(100000);
  VCI_ResetCAN(VCI_USBCAN2, 0, 1);
  usleep(100000);
}

void CANDispatch::Update(int ch)
{
  std::vector<CanStamped> frames;
  can->recv(frames, 0, ch); // timeout = 0，非阻塞模式，
                        // 调用 recv() 时，不会等待 CAN 总线有数据才返回，而是立即返回
  for (int i = 0; i < frames.size(); ++i)
  {
    // 接收 CAN 帧并回调处理函数
    uint32_t id = frames[i].ID & CAN_EFF_MASK;
    auto x = callbacks.find(id);
    if (x != callbacks.end())
    {
      // 跳转到对应的CAN ID处理函数
      x->second.callback(frames[i]);
      // 定时器清零
      x->second.tick = 0;
    }
  }

  std::cout << "Print buffer_send" << std::endl;
  if (ch == 0) {
    for(const auto &frame : buffer_send0) {
      std::cout << "Send frame ID: 0x" << std::hex << frame.ID
                << " DLC:" << std::dec << int(frame.DataLen)
                << " Data:";
      for(int i = 0; i < frame.DataLen; i++) {
        std::cout << std::hex << std::setw(2) << std::setfill('0')
                  << int(frame.Data[i]) << " ";
      }
      std::cout << std::endl;
    }

    can->send(buffer_send0, 1, 0);
    buffer_send0.clear();
    for (auto &i : callbacks)
    {
      if (i.second.interval <= 1)
        continue;
      i.second.tick += 1;
      if (i.second.tick > i.second.interval && i.second.timeout)
      {
        i.second.tick = 0;
        i.second.timeout();
      }
    }
  }
  else if (ch == 1) {
    for(const auto &frame : buffer_send1) {
      std::cout << "Send frame ID: 0x" << std::hex << frame.ID
                << " DLC:" << std::dec << int(frame.DataLen)
                << " Data:";
      for(int i = 0; i < frame.DataLen; i++) {
        std::cout << std::hex << std::setw(2) << std::setfill('0')
                  << int(frame.Data[i]) << " ";
      }
      std::cout << std::endl;
    }

    can->send(buffer_send1, 1, 1);
    buffer_send1.clear();
    for (auto &i : callbacks)
    {
      if (i.second.interval <= 1)
        continue;
      i.second.tick += 1;
      if (i.second.tick > i.second.interval && i.second.timeout)
      {
        i.second.tick = 0;
        i.second.timeout();
      }
    }
  }
}

void CANDispatch::Update2(int ch, int s_time)
{
  std::vector<CanStamped> send_frames;
  send_frames.resize(1);
  send_frames[0].ID = 0x80;
  send_frames[0].DataLen = 0;
  can->send(send_frames, 0, ch);
  usleep(s_time);
  std::vector<CanStamped> frames;
  can->recv(frames, 0, ch);
  for (int i = 0; i < frames.size(); ++i)
  {
    uint32_t id = frames[i].ID & CAN_EFF_MASK;
    auto x = callbacks.find(id);
    if (x != callbacks.end())
    {
      x->second.callback(frames[i]);
      x->second.tick = 0;
    }
  }

  if (ch == 0) {
    can->send(buffer_send0, 0, 0);
    buffer_send0.clear();
  }
  else if (ch == 1) {
    can->send(buffer_send1, 0, 1);
    buffer_send1.clear();
  }
  
  for (auto &i : callbacks)
  {
    if (i.second.interval <= 1)
      continue;
    i.second.tick += 1;
    if (i.second.tick > i.second.interval && i.second.timeout)
    {
      i.second.tick = 0;
      i.second.timeout();
    }
  }
}
void CANDispatch::send_msg(int ch)
{
  if (ch == 0) {
    can->send(buffer_send0, 1, 0);
    buffer_send0.clear();
  }
  else if (ch == 1) {
    can->send(buffer_send1, 1, 1);
    buffer_send1.clear();
  }

  for (auto &i : callbacks)
  {
    if (i.second.interval <= 1)
      continue;
    i.second.tick += 1;
    if (i.second.tick > i.second.interval && i.second.timeout)
    {
      i.second.tick = 0;
      i.second.timeout();
    }
  }
}

void CANDispatch::recv_msg(int ch)
{
  std::vector<CanStamped> frames;
  if (ch == 0) {
    do
    {
      can->recv(frames, 0, 0);
    } while (sizeof(frames) < 0);
  }
  else if (ch == 1) {
    do
    {
      can->recv(frames, 0, 1);
    } while (sizeof(frames) < 0);
  }

  for (int i = 0; i < frames.size(); ++i)
  {
    uint32_t id = frames[i].ID & CAN_EFF_MASK;
    auto x = callbacks.find(id);
    if (x != callbacks.end())
    {
      x->second.callback(frames[i]);
      x->second.tick = 0;
    }
  }
  for (auto &i : callbacks)
  {
    if (i.second.interval <= 1)
      continue;
    i.second.tick += 1;
    if (i.second.tick > i.second.interval && i.second.timeout)
    {
      i.second.tick = 0;
      i.second.timeout();
    }
  }
}

void CANDispatch::recv_msg(int ch, CanStamped &frame)
{
  std::vector<CanStamped> frames;
  if (ch == 0) {
    do
    {
      can->recv(frames, 0, 0);
    } while (sizeof(frames) < 0);
  }
  else if (ch == 1) {
    do
    {
      can->recv(frames, 0, 1);
    } while (sizeof(frames) < 0);
  }
  frame = frames[0];
}
